// Include files.
#include "../Internal/FpkAssert.hpp"
#include "StateMachineService.hpp"

// Using namespace.
using namespace std;

/**
 *  Constructor for StateMachineController.
 */
StateMachineService::StateMachineService(bool deactivateOnFinish) :
	m_deactivateOnFinish(deactivateOnFinish),
	m_stateInitial(NULL),
	m_stateCurrent(NULL),
	m_stateFinal(),
	m_statesOwned()
{
	Initialize();
}

/**
 *  Destructor for StateMachineController.
 */
StateMachineService::~StateMachineService()
{
	 for (list<State*>::iterator it = m_statesOwned.begin(); it != m_statesOwned.end(); it++)
	    delete (*it);
}

// Define empty function.
/**
 *  Override this function to add the States and trasitions to those States.
 */
void StateMachineService::Initialize()
{
}

/**
 *  Do not override this function.
 *  
 *  Override Initialize instead.
 */
void StateMachineService::HandleStart()
{
	Initialize();
	m_stateCurrent = m_stateInitial;
	
	// Initialize the inital state.
	m_stateCurrent->Start();
}

/**
 *  Overriders of this function should call this implimentation.
 *  
 *  You can do this by calling "StateMachineController::HandlePeriodic".
 */
void StateMachineService::HandlePeriodic()
{
	// Assert that the current state isn't null.
	FpkAssert(m_stateCurrent != NULL);
	
	if (&(m_stateCurrent->GetNextState()) != m_stateCurrent)
	{
		printf("State Machine: Transitioning to the next state.\n");
		m_stateCurrent = &(m_stateCurrent->GetNextState());
		printf("%p \n", m_stateCurrent);
		m_stateCurrent->Start();
	}
	
	if (m_deactivateOnFinish && m_stateCurrent == &m_stateFinal)
	{
		m_stateCurrent->Stop();
		Stop();
		return;
	}
	
	// HACK: We should better integrate state machine States into the Systematic framework.
	if (m_stateCurrent->GetIsStarted())
		m_stateCurrent->HandlePeriodic();
}

/**
 *  Overriders of this function should call this implimentation.
 *  
 *  You can do this by calling "StateMachineController::HandleContinuous".
 */
void StateMachineService::HandleContinuous()
{
	// HACK: We should better integrate state machine States into the Systematic framework.
	if (m_stateCurrent->GetIsStarted())
		m_stateCurrent->HandleContinuous();
}

/**
 *  Gets the very first State that runs.
 *  
 *  @returns The very first State that runs.
 */
State &StateMachineService::GetInitialState()
{
	return *m_stateInitial;
}

/**
 *  Sets the very first State that runs.
 *  
 *  @param state The desired State to run first.
 */
void StateMachineService::SetInitialState(State &state)
{
	m_stateInitial = &state;
}

/**
 *  Gets the final State.
 *  
 *  When the final State runs, the state machine is finished.
 *  
 *  @returns The final State.
 */
State &StateMachineService::GetFinalState()
{
	return m_stateFinal;
}

/**
 *  Gives the StateMachineController ownership of the specified State, thereby relieving the user of the need to delete it.
 *  
 *  @param state The State to be placed under owener of the StateMachineController.
 *               Behaviour is only defined if the state is dynamically allocated (e.g. "new State()").
 */
void StateMachineService::OwnState(State &state)
{
	m_statesOwned.push_back(&state);
}
